#include "fusion.hpp"


namespace faf
{
    float fusion::EKF_Fusion(float a, float b)
    {

        write_a(a); // 更新数据缓冲区
        write_b(b); // 更新数据缓冲区

        float a_data[BUFFER_SIZE];
        float b_data[BUFFER_SIZE];
        read_a(a_data); // 读取数据缓冲区
        read_b(b_data); // 读取数据缓冲区

        // 计算数据状态
        calculateLinearity(a_data, BUFFER_SIZE, &data_stauts_a);
        calculateLinearity(b_data, BUFFER_SIZE, &data_stauts_b);

        /*可以使用方差 也可以使用残差 这两者的区别在于方差是衡量数据变化的一种指标 而残差是衡量数据偏离真实值程度的一种指标
        在卡尔曼的公式中常用方差去卡尔曼增益 不过这里我稍微魔改了一下 先计算噪声比 然后计算卡尔曼增益 最后融合
        我个人的实验结果来说,方差的滤波效果似乎更好一些 残差的跟随性更好一些
        */
        float niose_a = data_stauts_a.residual / (data_stauts_a.residual + data_stauts_b.residual); // 计算噪声比
        float niose_b = data_stauts_b.residual / (data_stauts_a.residual + data_stauts_b.residual); // 计算噪声比

        // 预测更新
        estimatedError += processNoise;
        // 计算卡尔曼增益
        float kalmanGain = estimatedError / (estimatedError + measurementNoise);

        output = output + kalmanGain* ((a - output) * (niose_b ) + (b - output) * (niose_a )); // 融合

        // 误差更新
        estimatedError = (1 - kalmanGain) * estimatedError;


        return output;
    }
}

